﻿using System;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics
{
	// TODO : this constaint possibly causes bodies to "hop".  Probably due to satisfied flag issues and not taking into account elapsed time correctly.
	/// <summary>
	/// Constrains a relative position on one <see cref="RigidBody"/> to be fixed to 
	/// a relative position on another <see cref="RigidBody"/> with a hinge-type joint.
	/// </summary>
	public class HingeConstraint : PointToPointConstraint
	{
		private Vector3 axis0, axis1;

		/// <summary>
		/// Gets or sets the first <see cref="RigidBody"/> instance's relative axis to a shared hinge.
		/// </summary>
		public Vector3 RelativeAxis0 = Vector3.Up;

		/// <summary>
		/// Gets or sets the second <see cref="RigidBody"/> instance's relative axis to a shared hinge.
		/// </summary>
		public Vector3 RelativeAxis1 = Vector3.Down;

		/// <summary>
		/// Initialize a new instance of the <see cref="HingeConstraint"/> class.
		/// </summary>
		public HingeConstraint() : base() { }

		#region Constraint Members

		protected internal override void PreStep(float elapsed)
		{
			// Call inherited method.
			base.PreStep(elapsed);

			// Get the axes in world space.
#if QUATERNION_ORIENTATION
			this.axis0 = Vector3.Transform(this.RelativeAxis0, this.RigidBody0.Orientation);
			this.axis1 = Vector3.Transform(this.RelativeAxis1, this.RigidBody1.Orientation);
#else
#if INLINE
			this.axis0.X = this.RelativeAxis0.X * this.RigidBody0.Orientation.M11 + this.RelativeAxis0.Y * this.RigidBody0.Orientation.M21 + this.RelativeAxis0.Z * this.RigidBody0.Orientation.M31;
			this.axis0.Y = this.RelativeAxis0.X * this.RigidBody0.Orientation.M12 + this.RelativeAxis0.Y * this.RigidBody0.Orientation.M22 + this.RelativeAxis0.Z * this.RigidBody0.Orientation.M32;
			this.axis0.Z = this.RelativeAxis0.X * this.RigidBody0.Orientation.M13 + this.RelativeAxis0.Y * this.RigidBody0.Orientation.M23 + this.RelativeAxis0.Z * this.RigidBody0.Orientation.M33;

			this.axis1.X = this.RelativeAxis1.X * this.RigidBody1.Orientation.M11 + this.RelativeAxis1.Y * this.RigidBody1.Orientation.M21 + this.RelativeAxis1.Z * this.RigidBody1.Orientation.M31;
			this.axis1.Y = this.RelativeAxis1.X * this.RigidBody1.Orientation.M12 + this.RelativeAxis1.Y * this.RigidBody1.Orientation.M22 + this.RelativeAxis1.Z * this.RigidBody1.Orientation.M32;
			this.axis1.Z = this.RelativeAxis1.X * this.RigidBody1.Orientation.M13 + this.RelativeAxis1.Y * this.RigidBody1.Orientation.M23 + this.RelativeAxis1.Z * this.RigidBody1.Orientation.M33;
#elif REFERENCE
			Vector3.TransformNormal(ref this.RelativeAxis0, ref this.RigidBody0.Orientation, out this.axis0);
			Vector3.TransformNormal(ref this.RelativeAxis1, ref this.RigidBody1.Orientation, out this.axis1);
#else
			this.axis0 = Vector3.TransformNormal(this.RelativeAxis0, this.RigidBody0.Orientation);
			this.axis1 = Vector3.TransformNormal(this.RelativeAxis1, this.RigidBody1.Orientation);
#endif
#endif
			// TODO : Determine this instead of assuming unsatisfied.
			this.satisfied = false;
		}

		protected internal override bool Solve(float elapsed)
		{
			// Call inherited method.
			bool result = base.Solve(elapsed);

			// TODO : Check if this is necessary.
			// Exit if both rigid bodies are disabled.
			if ((this.RigidBody0.Flags & this.RigidBody1.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.Disabled)
			{
				return result;
			}

			// This flag holds the status of rotational correction.
			bool rotationResult = false;

			// Determine the relative rotation around the hinge.
			Vector3 relativeRotation = Vector3.Zero;
#if INLINE
			if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				float temp = this.axis0.X * this.RigidBody0.Rotation.X + this.axis0.Y * this.RigidBody0.Rotation.Y + this.axis0.Z * this.RigidBody0.Rotation.Z;
				relativeRotation.X = this.RigidBody0.Rotation.X - this.axis0.X * temp;
				relativeRotation.Y = this.RigidBody0.Rotation.Y - this.axis0.Y * temp;
				relativeRotation.Z = this.RigidBody0.Rotation.Z - this.axis0.Z * temp;
			}
			if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				float temp = this.axis1.X * this.RigidBody1.Rotation.X + this.axis1.Y * this.RigidBody1.Rotation.Y + this.axis1.Z * this.RigidBody1.Rotation.Z;
				relativeRotation.X -= this.RigidBody1.Rotation.X - this.axis1.X * temp;
				relativeRotation.Y -= this.RigidBody1.Rotation.Y - this.axis1.Y * temp;
				relativeRotation.Z -= this.RigidBody1.Rotation.Z - this.axis1.Z * temp;
			}
#elif REFERENCE
			if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				float temp0;
				Vector3.Dot(ref this.axis0, ref this.RigidBody0.Rotation, out temp0);

				Vector3 temp1;
				Vector3.Multiply(ref this.axis0, temp0, out temp1);
				Vector3.Subtract(ref this.RigidBody0.Rotation, ref temp1, out relativeRotation);
			}
			if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				float temp0;
				Vector3.Dot(ref this.axis1, ref this.RigidBody1.Rotation, out temp0);

				Vector3 temp1;
				Vector3.Multiply(ref this.axis1, temp0, out temp1);
				Vector3.Subtract(ref this.RigidBody1.Rotation, ref temp1, out temp1);

				Vector3.Subtract(ref relativeRotation, ref temp1, out relativeRotation);
			}
#else
			if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				Vector3 temp = this.axis0 * Vector3.Dot(this.axis0, this.RigidBody0.Rotation);
				relativeRotation = this.RigidBody0.Rotation - temp;
			}
			if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
			{
				Vector3 temp = this.axis1 * Vector3.Dot(this.axis1, this.RigidBody1.Rotation);
				relativeRotation -= this.RigidBody1.Rotation - temp;
			}
#endif
			// Determine the correction necessary to solve the rotational error.
			if (relativeRotation.LengthSquared() > MathExtension.Epsilon * MathExtension.Epsilon)
			{
#if INLINE
				Vector3 normal;
				Vector3.Normalize(ref relativeRotation, out normal);

				float denominator = 0.0f;
				if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					float x = normal.X * this.RigidBody0.inverseInertiaTensorWorld.M11 + normal.Y * this.RigidBody0.inverseInertiaTensorWorld.M21 + normal.Z * this.RigidBody0.inverseInertiaTensorWorld.M31;
					float y = normal.X * this.RigidBody0.inverseInertiaTensorWorld.M12 + normal.Y * this.RigidBody0.inverseInertiaTensorWorld.M22 + normal.Z * this.RigidBody0.inverseInertiaTensorWorld.M32;
					float z = normal.X * this.RigidBody0.inverseInertiaTensorWorld.M13 + normal.Y * this.RigidBody0.inverseInertiaTensorWorld.M23 + normal.Z * this.RigidBody0.inverseInertiaTensorWorld.M33;

					denominator = normal.X * x + normal.Y * y + normal.Z * z;
				}
				if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					float x = normal.X * this.RigidBody1.inverseInertiaTensorWorld.M11 + normal.Y * this.RigidBody1.inverseInertiaTensorWorld.M21 + normal.Z * this.RigidBody1.inverseInertiaTensorWorld.M31;
					float y = normal.X * this.RigidBody1.inverseInertiaTensorWorld.M12 + normal.Y * this.RigidBody1.inverseInertiaTensorWorld.M22 + normal.Z * this.RigidBody1.inverseInertiaTensorWorld.M32;
					float z = normal.X * this.RigidBody1.inverseInertiaTensorWorld.M13 + normal.Y * this.RigidBody1.inverseInertiaTensorWorld.M23 + normal.Z * this.RigidBody1.inverseInertiaTensorWorld.M33;

					denominator += normal.X * x + normal.Y * y + normal.Z * z;
				}

				if (denominator > MathExtension.Epsilon)
				{
					const float relaxation = 1.0f;
					float temp = (1.0f / denominator) * relaxation;
					
					relativeRotation.X = relativeRotation.X * temp;
					relativeRotation.Y = relativeRotation.Y * temp;
					relativeRotation.Z = relativeRotation.Z * temp;
					rotationResult = true;
				}
#elif REFERENCE
				Vector3 normal;
				Vector3.Normalize(ref relativeRotation, out normal);

				float denominator = 0.0f;
				if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					Vector3 temp;
					Vector3.TransformNormal(ref normal, ref this.RigidBody0.inverseInertiaTensorWorld, out temp);
					denominator = Vector3Extension.Dot(ref normal, ref temp);
				}
				if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					Vector3 temp;
					Vector3.TransformNormal(ref normal, ref this.RigidBody1.inverseInertiaTensorWorld, out temp);
					denominator += Vector3Extension.Dot(ref normal, ref temp);
				}

				if (denominator > MathExtension.Epsilon)
				{
					const float relaxation = 1.0f;
					Vector3.Multiply(ref relativeRotation, (1.0f / denominator) * relaxation, out relativeRotation);
					rotationResult = true;
				}
#else
				Vector3 normal = Vector3.Normalize(relativeRotation);

				float denominator = 0.0f;
				if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					denominator = Vector3.Dot(normal, Vector3.TransformNormal(normal, this.RigidBody0.inverseInertiaTensorWorld));
				}
				if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					denominator += Vector3.Dot(normal, Vector3.TransformNormal(normal, this.RigidBody1.inverseInertiaTensorWorld));
				}

				if (denominator > MathExtension.Epsilon)
				{
					const float relaxation = 1.0f;
					relativeRotation *= (1.0f / denominator) * relaxation;
					rotationResult = true;
				}
#endif
			}

			// Get the perpendicular of the the two relative axes.
#if INLINE
			float inverseElapsed = 1.0f / elapsed;
			Vector3 rotationalError = new Vector3()
			{
				X = -(this.axis0.Y * this.axis1.Z - this.axis0.Z * this.axis1.Y) * inverseElapsed,
				Y = -(this.axis0.Z * this.axis1.X - this.axis0.X * this.axis1.Z) * inverseElapsed,
				Z = -(this.axis0.X * this.axis1.Y - this.axis0.Y * this.axis1.X) * inverseElapsed
			};
#elif REFERENCE
			Vector3 rotationalError;
			Vector3.Cross(ref this.axis0, ref this.axis1, out rotationalError);
			Vector3.Negate(ref rotationalError, out rotationalError);
			Vector3.Divide(ref rotationalError, elapsed, out rotationalError);
#else
			Vector3 rotationalError = -Vector3.Cross(this.axis0, this.axis1) / elapsed;
#endif
			// Determine the correction necessary to solve the axis orientation error.
			if (rotationalError.LengthSquared() > MathExtension.Epsilon * MathExtension.Epsilon)
			{
#if INLINE
				Vector3 normal;
				Vector3.Normalize(ref rotationalError, out normal);

				float denominator = 0.0f;
				if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					float x = normal.X * this.RigidBody0.inverseInertiaTensorWorld.M11 + normal.Y * this.RigidBody0.inverseInertiaTensorWorld.M21 + normal.Z * this.RigidBody0.inverseInertiaTensorWorld.M31;
					float y = normal.X * this.RigidBody0.inverseInertiaTensorWorld.M12 + normal.Y * this.RigidBody0.inverseInertiaTensorWorld.M22 + normal.Z * this.RigidBody0.inverseInertiaTensorWorld.M32;
					float z = normal.X * this.RigidBody0.inverseInertiaTensorWorld.M13 + normal.Y * this.RigidBody0.inverseInertiaTensorWorld.M23 + normal.Z * this.RigidBody0.inverseInertiaTensorWorld.M33;

					denominator = normal.X * x + normal.Y * y + normal.Z * z;
				}
				if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					float x = normal.X * this.RigidBody1.inverseInertiaTensorWorld.M11 + normal.Y * this.RigidBody1.inverseInertiaTensorWorld.M21 + normal.Z * this.RigidBody1.inverseInertiaTensorWorld.M31;
					float y = normal.X * this.RigidBody1.inverseInertiaTensorWorld.M12 + normal.Y * this.RigidBody1.inverseInertiaTensorWorld.M22 + normal.Z * this.RigidBody1.inverseInertiaTensorWorld.M32;
					float z = normal.X * this.RigidBody1.inverseInertiaTensorWorld.M13 + normal.Y * this.RigidBody1.inverseInertiaTensorWorld.M23 + normal.Z * this.RigidBody1.inverseInertiaTensorWorld.M33;

					denominator += normal.X * x + normal.Y * y + normal.Z * z;
				}

				if (denominator > MathExtension.Epsilon)
				{
					const float relaxation = 1.0f;
					float temp = (1.0f / denominator) * relaxation;

					rotationalError.X = rotationalError.X * temp;
					rotationalError.Y = rotationalError.Y * temp;
					rotationalError.Z = rotationalError.Z * temp;
					rotationResult = true;
				}
#elif REFERENCE
				Vector3 normal;
				Vector3.Normalize(ref rotationalError, out normal);

				float denominator = 0.0f;
				if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					Vector3 temp;
					Vector3.TransformNormal(ref normal, ref this.RigidBody0.inverseInertiaTensorWorld, out temp);
					denominator = Vector3Extension.Dot(ref normal, ref temp);
				}
				if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					Vector3 temp;
					Vector3.TransformNormal(ref normal, ref this.RigidBody1.inverseInertiaTensorWorld, out temp);
					denominator += Vector3Extension.Dot(ref normal, ref temp);
				}

				if (denominator > MathExtension.Epsilon)
				{
					const float relaxation = 1.0f;
					Vector3.Multiply(ref rotationalError, (1.0f / denominator) * relaxation, out rotationalError);
					rotationResult = true;
				}
#else
				Vector3 normal = Vector3.Normalize(rotationalError);

				float denominator = 0.0f;
				if ((this.RigidBody0.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					denominator = Vector3.Dot(normal, Vector3.TransformNormal(normal, this.RigidBody0.inverseInertiaTensorWorld));
				}
				if ((this.RigidBody1.Flags & RigidBodyFlags.Static) != RigidBodyFlags.Static)
				{
					denominator += Vector3.Dot(normal, Vector3.TransformNormal(normal, this.RigidBody1.inverseInertiaTensorWorld));
				}

				if (denominator > MathExtension.Epsilon)
				{
					const float relaxation = 1.0f;
					rotationalError *= (1.0f / denominator) * relaxation;
					rotationResult = true;
				}
#endif
			}

			// No rotional correction was applied.  Just return the status of the linear constraints.
			if (!rotationResult)
			{
				return result;
			}

			// Apply the torque to correct the error in rotation.
#if INLINE
			Vector3 impulse = new Vector3()
			{
				X = rotationalError.X - relativeRotation.X,
				Y = rotationalError.Y - relativeRotation.Y,
				Z = rotationalError.Z - relativeRotation.Z
			};
			this.RigidBody0.ApplyTorqueImpulse(ref impulse);
			impulse.X = -impulse.X;
			impulse.Y = -impulse.Y;
			impulse.Z = -impulse.Z;
			this.RigidBody1.ApplyTorqueImpulse(ref impulse);
#elif REFERENCE
			Vector3 impulse;
			Vector3.Subtract(ref rotationalError, ref relativeRotation, out impulse);
			this.RigidBody0.ApplyTorqueImpulse(ref impulse);
			Vector3.Negate(ref impulse, out impulse);
			this.RigidBody1.ApplyTorqueImpulse(ref impulse);
#else
			Vector3 impulse = rotationalError - relativeRotation;
			this.RigidBody0.ApplyTorqueImpulse(impulse);
			impulse = -impulse;
			this.RigidBody1.ApplyTorqueImpulse(impulse);
#endif
			// Reset all collisions and constraints - a rotation change occurred.
			if (!result)
			{
				this.RigidBody0.CollisionGroup.Collisions.Reset();
				this.RigidBody0.Constraints.Reset();
				this.RigidBody1.CollisionGroup.Collisions.Reset();
				this.RigidBody1.Constraints.Reset();

				// Keep this constraint satisfied.
				this.satisfied = true;
			}

			// Some more processing may be necessary.
			return true;
		}

		#endregion
	}
}
